/*******************************************************************************
Copyright: Lippxin Microelectronics Co. Ltd.
FileName: FOC_PMSM.c
Author: Hunter
Date:   2024.08
Description: FOC 
Version: v1.0.1

Function List: 
1.DcBus_Ripple_suppress
2.FOC_Current_Controller
3.PMSM_Obs

*******************************************************************************/

/** Define to prevent recursive inclusion */
#define _FOC_PMSM_C_

/** Files includes */

#include "FOC_PMSM.h"
#include "mc_customize.h"
#include "pwm_gen.h"
#include "StateMachine.h"

/*************************************

**************************************/

/**
  * @brief Correct PWM duty cycle outputcalculation, based on the measured DC-bus voltage. 
  * @param sVAlBeIn  s16Vbus
  * @retval sVAlBe_Out
  */

void DcBus_Ripple_suppress(MCFLIB_2_ALBE_T_S16 *sVAlBeIn, Q15_t s16Vbus, MCFLIB_2_ALBE_T_S16 *sVAlBe_Out)
{
		#ifdef VBUS_COMPENSATE
			int32_t temp32;
		
			temp32 = ((int32_t)(sVAlBeIn->s16Alpha)) * ((int32_t)(Q15(0.866)));
			temp32 = (temp32/s16Vbus) << 1;  //0.866*2 = sqrt3
			sVAlBe_Out->s16Alpha = (int16_t)(temp32);	
	
			temp32 = ((int32_t)(sVAlBeIn->s16Beta)) * ((int32_t)(Q15(0.866))); //0.866*2 = sqrt3
			temp32 = (temp32/s16Vbus) << 1;
			sVAlBe_Out->s16Beta = (int16_t)(temp32);	
	
		#else
			sVAlBe_Out->s16Alpha = MLIB_Mul_Q15(sVAlBeIn->s16Alpha,FOC.s16KVbus)<<3;
			sVAlBe_Out->s16Beta = MLIB_Mul_Q15(sVAlBeIn->s16Beta,FOC.s16KVbus)<<3;
		#endif
	
//		temp32 = ((int32_t)(sVAlBeIn->s16Alpha)) * ((int32_t)28377);
//		_IQdivision(temp32,s16Vbus,temp32,hRemainder);
//		sVAlBe_Out->s16Alpha = temp32*2;

//		temp32 = ((int32_t)(sVAlBeIn->s16Beta)) * ((int32_t)28377);
//		_IQdivision(temp32,s16Vbus,temp32,hRemainder);
//		sVAlBe_Out->s16Beta = temp32*2;
}

/**
  * @brief  Sensorless SMO observer and calls
  *         PLL with the purpose of computing a new speed estimation and
  *         updating the estimated electrical angle
  * @param  Motor
  * @retval None
  */

void PMSM_Obs(FOC_TypeDef *pFOC)
{
    int16_t s16ThetaErr;
    sAngle_Trig     sAngle_pll;               /* the sin cos of pll angle*/

    /* SMO observer in AB system */
    pFOC->SMO.s16Ialpha = pFOC->sIAlBe.s16Alpha;
    pFOC->SMO.s16Ibeta = pFOC->sIAlBe.s16Beta;
    pFOC->SMO.s16Valpha = pFOC->sVAlBe.s16Alpha;
    pFOC->SMO.s16Vbeta = pFOC->sVAlBe.s16Beta;
    MLIB_SMO(&pFOC->SMO); 
	
    sAngle_pll = MLIB_SinCos(pFOC->s16ThetaPLL);
	
    /* Calculate the input of PLL   */
    s16ThetaErr = MLIB_Mul_Q15(-pFOC->SMO.s16Ealpha_est, sAngle_pll.s16Cos) - MLIB_Mul_Q15(pFOC->SMO.s16Ebeta_est, sAngle_pll.s16Sin);
		
	/* Calculate the motor angle  */
    pFOC->s16ThetaPLL = MLIB_PLL(s16ThetaErr,&pFOC->sPLL);
	  
	/* update the motor angle  */
		pFOC->s16CmdTheta = pFOC->s16ThetaPLL + pFOC->s16ErrTheta;
    pFOC->s16CmdTheta = pFOC->s16CmdTheta + pFOC->s16ThetaComp;
	
    /* PLL calculation */
    pFOC->s16CmdTheta_Update = pFOC->s16CmdTheta + MLIB_Mul_Q15(pFOC->s16SpdFilt, SPEED_TO_THETA);
    pFOC->SMO.s16Speed = pFOC->sPLL.s32Speed >> 16;
}

/**
  * @brief It executes the core of FOC drive that is the controllers for Iqd
  *        currents regulation. It must be called periodically
  *        when new motor currents have been converted
  * @param Motor.
  * @retval None
  */

void FOC_Current_Controller(FOC_TypeDef *pFOC)
{
		int32_t s32Temp;
		int16_t s16Temp;
	
	  /* Calculate the sincos of  s16CmdTheta */
    pFOC->sAngle_cmd = MLIB_SinCos(pFOC->s16CmdTheta);
	
	  /* PARK  transform */
    MCFLIB_Park_S16(&pFOC->sIAlBe, &pFOC->sAngle_cmd, &pFOC->sIdq);
	
	#ifdef FAST_BOOT_METHOD
	
		s16Temp = MLIB_Mul_Q15(USER.s16DutyLimitCmd, pFOC->s16VbusAvg);
		pFOC->s16VMAX = MLIB_Mul_Q15(s16Temp, Q15(0.6667)); //Amplitude invariant transformation

		if(eM1_RunSubState == RunState_Startup)
		{
			Fast_Boot_Current_Controller(&FOC);
		}
		else
		{
				pFOC->CurID.qOutMax = pFOC->s16VMAX;
				pFOC->CurID.qOutMin = -pFOC->CurID.qOutMax;
				
				pFOC->CurID.qInRef = pFOC->sIdq_cmd.s16D;
				pFOC->CurID.qInMeas = pFOC->sIdq.s16D;
				CalcPI(&pFOC->CurID);
				pFOC->sVdq.s16D = pFOC->CurID.qOut;
			
				/* Calculate the PI saturation for Vq    */	
				s32Temp = pFOC->s16VMAX * pFOC->s16VMAX - pFOC->sVdq.s16D * pFOC->sVdq.s16D;
				pFOC->CurIQ.qOutMax =  Sqrt_Q15(s32Temp);
				pFOC->CurIQ.qOutMin = -pFOC->CurIQ.qOutMax;
				
				pFOC->CurIQ.qInRef = pFOC->sIdq_cmd.s16Q;
				pFOC->CurIQ.qInMeas = pFOC->sIdq.s16Q;
				CalcPI(&pFOC->CurIQ);
				pFOC->sVdq.s16Q = pFOC->CurIQ.qOut;
		}
	
	#else
	
		/* Calculate the PI saturation for Vd    */
		s16Temp = MLIB_Mul_Q15(USER.s16DutyLimitCmd, pFOC->s16VbusAvg);
		pFOC->s16VMAX = MLIB_Mul_Q15(s16Temp, Q15(0.6667)); //Amplitude invariant transformation
		pFOC->CurID.qOutMax = pFOC->s16VMAX;
		pFOC->CurID.qOutMin = -pFOC->CurID.qOutMax;
		
		pFOC->CurID.qInRef = pFOC->sIdq_cmd.s16D;
		pFOC->CurID.qInMeas = pFOC->sIdq.s16D;
		CalcPI(&pFOC->CurID);
		pFOC->sVdq.s16D = pFOC->CurID.qOut;
	
		/* Calculate the PI saturation for Vq    */	
		s32Temp = pFOC->s16VMAX * pFOC->s16VMAX - pFOC->sVdq.s16D * pFOC->sVdq.s16D;
		pFOC->CurIQ.qOutMax =  Sqrt_Q15(s32Temp);
		pFOC->CurIQ.qOutMin = -pFOC->CurIQ.qOutMax;
		
		pFOC->CurIQ.qInRef = pFOC->sIdq_cmd.s16Q;
		pFOC->CurIQ.qInMeas = pFOC->sIdq.s16Q;
		CalcPI(&pFOC->CurIQ);
		pFOC->sVdq.s16Q = pFOC->CurIQ.qOut;
		
	#endif
	
		/* Update the sincos for invpark */
		pFOC->sAngle_cmd_update = pFOC->sAngle_cmd;
	
		/* Inv PARK transform*/
    MCFLIB_InvPark_S16(&pFOC->sVdq, &pFOC->sAngle_cmd_update, &pFOC->sVAlBe);
		
		/* Elimination of the DC-bus voltage ripple  */
    DcBus_Ripple_suppress(&pFOC->sVAlBe, pFOC->s16VbusAvg, &pFOC->sVAlBe_cmd);
}
